#!/usr/bin/python
from ctypes import *

class MiniBotControlLib:
   '导入动态库'
   lib = cdll.LoadLibrary("./MiniBotControlLib.dll")
 
   #设置接收数据超时时间
   def SetRecvTimeOut(self,nNetTimeout):
    r=self.lib.SetRecvTimeOut(nNetTimeout)
    return r
    
   #扫描在线机器人设备
   def ScanRobots(self):
    charPP = POINTER(c_char_p)
    ipPPvalue = c_char_p(5)
    ipPP = charPP(ipPPvalue)
    int_p = POINTER(c_int)
    countObject = c_int(4)
    count = int_p(countObject)
    num = 0
    num = self.lib.ScanRobots(ipPP,count)
    robotsLs = []
    for i in range(num):
       #print("Robot=" + str(ipPP[i]))
       robotsLs.append(str(ipPP[i],'utf-8'))

    return robotsLs
 

    #连接目标机器人设备
   def Connect(self,ip, port):
     r = self.lib.Connect(c_char_p(ip.encode('utf-8')),c_int(port))
     return r
   
    #连接机器人（带超时设置）
   def ConnectWithTimeOut(self,ip, port,nNetTimeout):
     r = self.lib.ConnectWithTimeOut(c_char_p(ip.encode('utf-8')),c_int(port),c_int(nNetTimeout))
     return r

   #断开目标机器人设备
   def Disconnect(self):
      r=self.lib.Disconnect()
      return r


   #扫描机器人底座连接的所有模块ID【返回Id 及 对应位置 列表】
   def ScanId(self,startId, endId):
     self.lib.ScanId.argtypes = [c_ubyte,c_ubyte,POINTER(c_ubyte),POINTER(c_ubyte)]
     self.lib.ScanId.restype = c_int
     id = c_ubyte(0)
     position = c_ubyte(0)
     num = self.lib.ScanId(startId,endId,id,position)
     id_p = pointer(id)
     position_p = pointer(position)
   
     print("Total Ids Count=" + str(num))
     idLs = []
     posLs = []
     for i in range(num):
         print("Id=" + str(id_p[i]))
         idLs.append(id_p[i])
     for i in range(num):
         print("Position=" + str(position_p[i]))
         posLs.append(position_p[i])
     return idLs,posLs


   #获取指定ID关节模块的位置角度值
   def GetPosition(self,id):
      cid = c_ubyte(id)
      self.lib.GetPosition.restype = c_int
      angle = self.lib.GetPosition(cid)
      return angle

   #获取指定ID关节模块的速度信息-【关节模块处于速度模式】
   def GetRunningSpeed(self,id):
      cid = c_ubyte(id)
      self.lib.GetPosition.restype = c_int
      speed = self.lib.GetRunningSpeed(cid)
      return speed

   #获取指定ID关节模块的速度信息-【关节处于位置模式】
   def GetPositionSpeed(self,id):
      cid = c_ubyte(id)
      self.lib.GetPosition.restype = c_int
      speed = self.lib.GetPositionSpeed(cid)
      return speed


   #设置关节模块位置角度（位置模式）
   def SetPosition(self,id,angle): 
      r = self.lib.SetPosition(c_ubyte(id),c_double(angle))
      return r
      
   #设置关节模块以指定速度运行到指定位置角度
   def SetPositionWithSpeed(self,id,angle,speed):
      r = self.lib.SetPositionWithSpeed(c_ubyte(id),c_double(angle),c_int(speed))
      return r

   # 设置指定ID关节模块的运行速度，会自动将关节模块设置为速度模式。该设置仅对I模块有效
   def SetRunningSpeed(self,id,speed):
      r = self.lib.SetRunningSpeed(c_ubyte(id),c_int(speed))
      return r

   #  设置关节模块运行到指定角度的速度
   def SetPositionSpeed(self,id,speed):
      r = self.lib.SetPositionSpeed(c_ubyte(id),c_int(speed))
      return r


   # 设置指定ID关节是否使能：id,isEnable(0：关；1：开启使能)
   def EnableTorque(self,id,isEnable):
      r = self.lib.EnableTorque(c_ubyte(id),bool(isEnable))
      return r


   #设置关节模块模式：mode(0位置,1速度,2开环调速度模式)
   def SetMotorMode(self,id,mode):
      r = self.lib.SetMotorMode(c_ubyte(id),c_int(mode))
      return r

  #id = c_ubyte*2
#idarry = id(25,39)
#position = c_double*2
#positionarry = position(150,60)
#count = c_int(2)
#res1 = BW.SetPositionSync(idarry,positionarry,count)
#if res1:
#    print("设置成功")
#else:
#    print("设置失败")

   # 同步设置多个关节模块的位置
   def SetPositionSync(self,ids,angles):
      size=len(ids)
      idLs =(c_ubyte*size)()
      posLs=(c_double*size )()
      for i in range(size):
         idLs[i]=(c_ubyte(ids[i]))
         posLs[i]=(c_double(angles[i]))    
      r = self.SetPositionSync(idLs,posLs,c_int(size))
      return r

   #设置模块的ID，设置的ID应满足以下ID段，否则使用时可能会造成硬件损伤
   def SetMotoId(self,oldId,newId):
      r = self.lib.SetMotoId(c_ubyte(oldId),c_ubyte(newId))
      return r

   #设置模块是否锁定不可写id,isLock(0：锁关闭；1：锁开启)
   def SetMotorLocked(self,id,isLock):
      r = self.lib.SetMotorLocked(c_ubyte(id),bool(isLock))
      return r


   #设置当前连接的机器人设备的连接模式为STA模式，即连接到公网WIFI热点 注：不支持5G网络
   def SetSTAMode(self,ssid,pwd):
      r = self.lib.SetSTAMode(bytes(ssid),bytes(pwd))
      return r

   #设置当前连接的机器人设备的连接模式为AP模式，即将当前设备设置为可连接的热点
   def SetAPMode(self,ssid,pwd):
      r = self.lib.SetAPMode(bytes(ssid),bytes(pwd))
      return r

   #设置小移动平台灯带模式（如果存在）
   def SetLEDBel(self,val):
      self.lib.SetLEDBel.restype = c_int
      r = self.lib.SetLEDBel(c_int(val))
      return r

   #读取外部设备数据(需连接传感器模块)
   def GetDevData(self,id,ioPort):
      r = self.lib.GetDevData(c_ubyte(val),c_ubyte(ioPort))
      return r

   #设置传感器数据
   def SetDevData(self,id,ioPort,data):
       dataLen=len(data)
       data_array=(c_ubyte*size)()
       for i in range(size):
        data_array[i]=(c_ubyte(data[i]))
       r = self.lib.SetDevData(c_ubyte(id),c_ubyte(ioPort),data_array,c_int(dataLen))
       return r

   #从数据缓冲区中读取设备数据（例如：红外按钮）
   def ReadDevData(self,id,ioPort):
      r = self.lib.ReadDevData(c_ubyte(id),c_ubyte(ioPort))
      return r

   #设置模块高压保护值
   def SetMotoHighVoltageProtection(self,id,value):
      r = self.lib.SetMotoHighVoltageProtection(c_ubyte(id),c_ubyte(value))
      return r

   #设置模块低压保护值
   def SetMotoLowVoltageProtection(self,id,value):
      r = self.lib.SetMotoLowVoltageProtection(c_ubyte(id),c_ubyte(value))
      return r

   #设置模块的最大角度限制
   def SetMotoMaxAngle(self,id,angle):
      r = self.lib.SetMotoMaxAngle(c_ubyte(id),c_int(angle))
      return r

   #设置模块的最小角度限制
   def SetMotoMinAngle(self,id,angle):
      r = self.lib.SetMotoMinAngle(c_ubyte(id),c_int(angle))
      return r

   #设置PWM(运行时间)
   def SetPWM(self,id,value):
      r = self.lib.SetPWM(c_ubyte(id),c_int(value))
      return r

   #获取PWM(运行时间)
   def GetPWM(self,id):
      int_Point = POINTER(c_int)
      pwm = c_int(0)
      pwm_p = int_Point(pwm)
      r = self.lib.GetPWM(c_ubyte(id),pwm_p)
      return pwm_p.contents

   #设置位置环P 比例系数
   def SetRingP(self,id,value):
      r = self.lib.SetPWM(c_ubyte(id),c_ubyte(value))
      return r

   #设置位置环D 微分系数
   def SetRingD(self,id,value):
      r = self.lib.SetPWM(c_ubyte(id),c_ubyte(value))
      return r

   #设置位置环I 积分系数
   def SetRingI(self,id,value):
      r = self.lib.SetPWM(c_ubyte(id),c_ubyte(value))
      return r

   #设置卸载条件
   def SetProtectConditions(self,id,isVoltage,isSensor,isTemperature,isElectricity,isAngle,isOverload):
      r = self.lib.SetProtectConditions(c_ubyte(id),bool(isVoltage),bool(isSensor),bool(isTemperature),bool(isElectricity),bool(value),bool(value))
      return r


   #设置LED报警条件
   def SetLedConditions(self,id,isVoltage,isSensor,isTemperature,isElectricity,isAngle,isOverload):
      r = self.lib.SetLedConditions(c_ubyte(id),bool(isVoltage),bool(isSensor),bool(isTemperature),bool(isElectricity),bool(value),bool(value))
      return r